import rclpy
import os
import threading
import time
import math
from rclpy.node import Node
from rclpy.duration import Duration
from std_msgs.msg import Bool
from std_srvs.srv import Empty
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist,PoseWithCovariance,Quaternion,TransformStamped
from tf2_ros import TransformBroadcaster
from tf_transformations import quaternion_from_euler

from robot_msgs.msg import Hardware

from .stm32_interface import Stm32SerialInterface

class HardwareBridge(Node):
    def __init__(self):
        node_name = "hardware_bridge_node"
        super().__init__(node_name)

        # 
        self.angular_speed = 0 # 角速度
        self.linear_speed = 0 # 线速度
        # 初始化位姿
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # 上一次累计编码器值（单位：mm）
        self.last_l_enc = 0
        self.last_r_enc = 0

        # 上次编码器时间
        self.last_enc_time = self.get_clock().now()

        # 
        self.last_speed_time = self.get_clock().now()
        
        # 初始化接口
        self.DeclareParams()
        self.InitInterfaces()

        # 线程管理
        self.stop_event = threading.Event()
        self.serial_thread = threading.Thread(target=self.serial_read_loop)
        self.serial_thread.daemon = True
        self.serial_thread.start()

        # 定时发布
        self.create_timer(0.02, self.pub_odom_timer)  # 50Hz
        self.create_timer(0.02, self.pub_pos_odom_timer) 
        self.create_timer(0.02, self.pub_hardware_state_timer)
        

        self.create_timer(0.05, self.write_speed_to_stm32_timer)
    
    
    def DeclareParams(self):
        '''
        声明参数&获取参数
        '''
        # 参数声明
        self.declare_parameter('serial_port', '/dev/ttyACM0') # '/dev/ttyS0' '/dev/ttyUSB0' '/dev/ttyACM0'
        self.declare_parameter('serial_baud_rate', 115200)
        self.declare_parameter('track_width', 0.440) # 轮距，单位m
        self.declare_parameter('wheel_radius', 80) # 轮半径
        self.declare_parameter('wheel_diameter', 80) # 轮直径
        self.declare_parameter('base_link_frame','base_link') # 机器人坐标系
        self.declare_parameter('odom_frame','odom') # 里程计坐标系
        # 获取参数
        ## 底盘参数
        self.track_width = self.get_parameter('track_width').value
        self.wheel_radius = self.get_parameter('wheel_radius').value
        self.wheel_diameter = self.get_parameter('wheel_diameter').value
        ## 底盘串口配置信息
        self.serial_port = self.get_parameter('serial_port').value
        self.serial_baud = self.get_parameter('serial_baud_rate').value
        ## 坐标系
        self.base_link_frame = self.get_parameter('base_link_frame').value
        self.odom_frame = self.get_parameter('odom_frame').value
    
    def InitInterfaces(self):
        '''
        初始化接口
        '''
        # 初始化广播器
        self.tf_broadcaster = TransformBroadcaster(self)
        # 订阅话题
        ## 订阅速度控制命令
        self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_callback, 10)
        ## 软急停
        self.create_subscription(Bool, 'vel_en', self.vel_en_callback, 1)
        ## 外设控制
        self.create_subscription(Hardware, 'cmd_utils', self.cmd_utils_callback, 1)

        # 发布话题
        ## 发布里程计信息
        self.odom_pub = self.create_publisher(Odometry, '/odom', 10)
        ## 发布机器人位置信息
        self.pos_odom_pub = self.create_publisher(PoseWithCovariance,'/pos_odom',10)
        ## 发布硬件状态
        self.hardware_state_pub = self.create_publisher(Hardware, '/hardware_state', 10)
              
      
        # 服务
        
        # 初始化串口
        self.stm32 = None
        self.InitSerial()
        
    
    def InitSerial(self):
        '''
        初始化串口
        '''
        self.stm32 = Stm32SerialInterface(self, self.serial_port, self.serial_baud)
    
    def destroy_node(self):
        self.get_logger().info("Shutting down hardware interface node...")
        # 自己的清理逻辑
        try:
            self.stop_event.set()
            if self.serial_thread.is_alive():
                self.serial_thread.join(timeout=2.0)
        except Exception as e:
            self.get_logger().warn(f"Failed to join serial thread: {e}")

        try:
            self.stm32.serial_close()
        except Exception as e:
            self.get_logger().warn(f"Failed to close serial port: {e}")

        # ROS 2 的底层清理
        super().destroy_node()
      

    def serial_read_loop(self):
        #使用后台线程持续读取串口，ROS 只负责发布数据
        rate = self.create_rate(100)
        while rclpy.ok() and not self.stop_event.is_set():
            rate.sleep()
            try:
                if not self.stm32.serial_is_open:
                    self.get_logger().warn("Reconnecting serial...")
                    self._init_serial()
                    rate.sleep()
                    continue
                
                if self.stm32.read_from_serial():
                    pass
                        
            except Exception as e:
                self.get_logger().error(f"Serial error: {e}")
                rate.sleep()
    

    #############
    #订阅
    #############
    def cmd_vel_callback(self, msg:Twist):
        self.last_speed_time = self.get_clock().now()
        self.linear_speed = msg.linear.x 
        self.angular_speed = msg.angular.z 
    
    def vel_en_callback(self, msg:Bool):
        pass
        # self.linear_speed = 0
        # self.angular_speed = 0
    
    def cmd_utils_callback(self, msg:Hardware):
        pass

       
        
    #############
    #服务
    #############
  


    #############
    #发布
    #############

    # def pub_motor_encoder_timer(self):
    #     '''
    #     发布电机编码器信息
    #     '''
    #     try:
    #         if not self.stm32.serial_is_open:
    #             return
            
    #         motor_encoder = self.stm32.motor_encoder_counts
    #         get_data_time = self.stm32.motor_encoder_time
            
    #         msg = MotorEncoder()
    #         msg.stamp = self.get_clock().now().to_msg()
    #         msg.uart_stamp = get_data_time[0]
    #         msg.motor_encoder_stamp = get_data_time[1]
    #         msg.motor_encoder_l = motor_encoder[0]
    #         msg.motor_encoder_r = motor_encoder[1]
        
    #         self.motor_encoder_pub.publish(msg)
                
    #     except Exception as e:
    #         self.get_logger().error(f"Pub motor encoder error: {e}")

    def pub_odom_timer(self):
        try:
            if not self.stm32.serial_is_open:
                return
            # 获取左右电机编码器累计数据（单位：mm）
            l_enc, r_enc = self.stm32.motor_encoder_counts 
            
            # 差值
            delta_l = l_enc - self.last_l_enc
            delta_r = r_enc - self.last_r_enc
            self.last_l_enc = l_enc
            self.last_r_enc = r_enc

            # 时间间隔
            now_time = self.get_clock().now()
            dt = (now_time-self.last_enc_time).nanoseconds / 1e9
            self.last_enc_time = now_time

            # 转为米
            delta_l_m = delta_l / 1000.0
            delta_r_m = delta_r / 1000.0

            # 位姿增量计算
            delta_s = (delta_l_m + delta_r_m) / 2.0
            delta_theta = (delta_r_m - delta_l_m) / self.track_width

            # 位姿更新
            self.x += delta_s * math.cos(self.theta + delta_theta / 2.0)
            self.y += delta_s * math.sin(self.theta + delta_theta / 2.0)
            self.theta += delta_theta

            # 速度估算
            v = delta_s / dt if dt > 0 else 0.0
            w = delta_theta / dt if dt > 0 else 0.0

            # 发布里程计消息
            odom = Odometry()
            odom.header.stamp = self.get_clock().now().to_msg()
            odom.header.frame_id = "odom"
            odom.child_frame_id = "base_link"

            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0.0
            q = quaternion_from_euler(0, 0, self.theta)
            odom.pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])

            odom.twist.twist.linear.x = v
            odom.twist.twist.angular.z = w

            self.odom_pub.publish(odom)
            
            # 广播TF
            t = TransformStamped()
            t.header.stamp = self.get_clock().now().to_msg()
            t.header.frame_id = "odom"
            t.child_frame_id = "base_link"
            t.transform.translation.x = self.x
            t.transform.translation.y = self.y
            t.transform.translation.z = 0.0
            t.transform.rotation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
            self.tf_broadcaster.sendTransform(t)
   
        except Exception as e:
            self.get_logger().error(f"Pub odom error: {e}")

    def pub_pos_odom_timer(self):
        pass
    def pub_hardware_state_timer(self):
        pass

    #############
    #写入
    #############
    def write_speed_to_stm32_timer(self):
        now_speed_time = self.get_clock().now()

        if now_speed_time > self.last_speed_time + Duration(seconds=0.5):
            self.linear_speed = 0
            self.angular_speed = 0

        # 差速模型计算左右轮速度
        left_speed = self.linear_speed - (self.track_width * self.angular_speed / 2.0)  # m/s
        right_speed = self.linear_speed + (self.track_width * self.angular_speed / 2.0) # m/s

        # 转换为整型（限制在 int16 范围）
        left_speed_int = max(min(int(left_speed * 1000), 32767), -32768)   # mm/s
        right_speed_int = max(min(int(right_speed * 1000), 32767), -32768) # mm/s

        try:
            if not self.stm32.serial_is_open:
                self.get_logger().warn("Serial port not open.")
                return
            
            self.stm32.send_encoder_data(left_speed_int, right_speed_int)
            self.get_logger().info(f"Send speed: {left_speed_int}, {right_speed_int}")
          
        except Exception as e:
            self.get_logger().error(f"Write speed error: {e}")